package Lab1;

import se.lth.control.DoublePoint;
import se.lth.control.realtime.*;
//import se.lth.control.realtime.IOChannelException;
import se.lth.control.realtime.Semaphore;
import se.lth.cs.realtime.FixedPriorityException;
import se.lth.cs.realtime.RTThread;

public class Regul extends RTThread {
	public static final int OFF = 0;
	public static final int BEAM = 1;
	public static final int BALL = 2;
	public static final int REF = 3;
	
	//
	// Set sendToProcess to true if you want the
	// methods to send data to the process
	//
	private static final boolean sendToProcess = true;
	
	private PI inner = new PI("PI");
	private PID outer = new PID("PID");

	private AnalogIn analogInAngle; 
	private AnalogIn analogInPosition; 
	private AnalogOut analogOut;

	private ReferenceGenerator referenceGenerator;
	private OpCom opcom;

	private int priority;
	private boolean WeShouldRun = true;
	private long starttime;
	private Semaphore mutex; // used for synchronization at shut-down

	private ModeMonitor modeMon;
	private RegulMon regulMon;

	private double yAngle,yPos,y,yref,u,xref,xPos,uOuter;
	
	// Inner monitor class
	class ModeMonitor {
		private int mode;

		public ModeMonitor(int mode){
			this.mode=mode;
		}
		// Synchronized access methods
		public synchronized void setMode(int newMode) {
			mode = newMode;
		}

		public synchronized int getMode() {
			return mode;
		}
	}

	public Regul(int pri, RegulMon regulMon) {
		priority = pri;
		mutex = new Semaphore(1);
		try {
			analogInAngle = new AnalogIn(1);
			analogInPosition = new AnalogIn(0);
			if(sendToProcess)
				analogOut = new AnalogOut(0);
		} catch (Exception e) {//IOChannelException e) { 
			System.out.print("Error: IOChannelException: ");
			System.out.println(e.getMessage());
		}
		modeMon = new ModeMonitor(REF);
		this.regulMon=regulMon;
	}

	public void setOpCom(OpCom opcom) {
		// Written by you
		this.opcom = opcom;
	}

	public void setRefGen(ReferenceGenerator referenceGenerator){
		// Written by you
		this.referenceGenerator=referenceGenerator;
	}

	// Called in every sample in order to send plot data to OpCom
	private void sendDataToOpCom(double yref, double y, double u) {
		double x = (double)(System.currentTimeMillis() - starttime) / 1000.0;
		DoublePoint dp = new DoublePoint(x,u);
		PlotData pd = new PlotData(x,yref,y);
		opcom.putControlDataPoint(dp);
		opcom.putMeasurementDataPoint(pd);
	}

	public synchronized void setInnerParameters(PIParameters p) {
		// Written by you
		inner.setParameters(p);
	} 

	public synchronized PIParameters getInnerParameters() {
		// Written by you
		return inner.getParameters();
	}

	public synchronized void setOuterParameters(PIDParameters p) {
		// Written by you
		outer.setParameters(p);
	}

	public synchronized PIDParameters getOuterParameters(){
		// Written by you
		return outer.getParameters();
	}

	public void setOFFMode(){
		// Written by you
		modeMon.setMode(OFF);
	}

	public void setBEAMMode(){
		// Written by you
		modeMon.setMode(BEAM);
	}

	public void setBALLMode(){
		// Written by you
		modeMon.setMode(BALL);
	}

	public void setRefMode(){
		// Written by you
		modeMon.setMode(REF);
	}
	
	public int getMode(){
		// Written by you
		return modeMon.getMode();
	}

	// Called from OpCom when shutting down
	public synchronized void shutDown() {
		WeShouldRun = false;
		mutex.take();
		try {
			if(sendToProcess)
				analogOut.set(0.0);
		} catch (Exception e) {//IOChannelException x) {
		}
	}

	private double limit(double v, double min, double max) {
		if (v < min) {
			v = min;
		} else {
			if (v > max) {
				v = max;
			}
		}
		return v;
	}

	public void run() {
		long duration;
		long t = System.currentTimeMillis();
		starttime = t;

		try {
			setPriority(priority);
		} catch (FixedPriorityException e1) {
			e1.printStackTrace();
		}
		mutex.take();
		setRefMode();
		while (WeShouldRun) {
			switch (modeMon.getMode()) {
			case OFF: {
				// Code for the OFF mode. 
				// Written by you.
				// Should include resetting the controllers
				// Should include a call to sendDataToOpCom  
				inner.reset();
				outer.reset();
				try{
//					System.out.println("[Regul]: angleFromCam=" + regulMon.getPos() + "angleFromProcess=" + analogInPosition.get());
					//System.out.println("[Regul]: angleFromCam=" + regulMon.getAng() + "angleFromProcess=" + analogInAngle.get());
					if(sendToProcess)
						analogOut.set(0);
				}catch(Exception e) {//IOChannelException x){
				
				}
				
				//System.out.println("[Regul]: "+ "AngleCam = " + regulMon.getAng() + ", AngleProc=" + analogInAngle.get() + ", posCam=" + regulMon.getPos());
				sendDataToOpCom(0,0,0);
				break;
			}
			case BEAM: {
				// Code for the BEAM mode
				// Written by you.
				// Should include a call to sendDataToOpCom
				try{
				//regulMon.setAng(analogInAngle.get());
				y = regulMon.getAng();
				}catch(Exception e) {//IOChannelException x){
				}
				yref = referenceGenerator.getRef();
				//yref = regulMon.getPosRef();
				u = limit(inner.calculateOutput(y,yref),-10,10);
				//System.out.println("[Regul]: yref = " + yref + ", y = " + y + ", yAngle=" + yAngle + ", yAngleProc=" + analogInAngle.get() + ", u =" + u);
				try{
					if(sendToProcess)
						analogOut.set(u);
				}catch(Exception e) {//IOChannelException x){
				}
				inner.updateState(u);
				sendDataToOpCom(yref,y,u);
				break;
			}
			case BALL: {
				// Code for the BALL mode
				// Written by you.
				// Should include a call to sendDataToOpCom 
				try{
					//yAngle = analogInAngle.get();
					//yPos = analogInPosition.get();
				}catch(Exception e) {//IOChannelException x){
				}
				
				yAngle = regulMon.getAng();
				//yAngle = analogInAngle.get();
				xPos = regulMon.getPos();
				//xPos = analogInPosition.get();
				//regulMon.setPosRef(referenceGenerator.getRef());
				//xref = regulMon.getPosRef();
				xref = referenceGenerator.getRef();
				//xref = regulMon.getPosRef();
				uOuter = outer.calculateOutput(xPos,xref);
				u = limit(inner.calculateOutput(yAngle,uOuter),-10,10);
				try{
					if(sendToProcess){
						analogOut.set(u);
						//System.out.println("[Regul]: xref=" + xref + ", xPosCam=" + xPos + ", xPosProc="+ analogInPosition.get() + ", yAngle=" + yAngle + ", yAngleProc=" + analogInAngle.get() + ", u=" + u);
					}
				}catch(Exception e) {//IOChannelException x){
				}
				outer.updateState(uOuter);
				inner.updateState(u);
				sendDataToOpCom(xref,xPos,u);
				break;
			}
			case REF: {
				// Code for the REF mode.
				// REF mode is used to be 
				// able to control the 
				// beams angle "manually"
				// in your code.
				try{
					regulMon.setAng(analogInAngle.get());
					yref = regulMon.getAngleRef();
					y = regulMon.getAng();
					u = limit(inner.calculateOutput(y,yref),-10,10);
					//System.out.println("[Regul]: yref=" + yref + ", y=" + y + ", u=" + u);
					if(sendToProcess)
						analogOut.set(u);
				}catch(Exception e) {//IOChannelException x){
				
				}
				inner.updateState(u);
				sendDataToOpCom(yref,y,u);
				break;
			}
			default: {
				System.out.println("Error: Illegal mode.");
				break;
			}
			}
			// sleep
			t = t + inner.getHMillis();
			duration = t - System.currentTimeMillis();
			if (duration > 0) {
				try {
					sleep(duration);
				} catch (InterruptedException x) {
				}
			}
		}
		mutex.give();
	}
}
